/*
 * @Author: liu Shihao
 * @Date: 2022-06-12 20:54:47
 * @LastEditors: liu Shihao
 * @LastEditTime: 2022-06-16 21:50:46
 * @FilePath: \bearpi-hm_nano\applications\BearPi\BearPi-HM_Nano\sample\robot\src\scs15_control.c
 * @Description: 空闲中断
 * Copyright (c) 2022 by ${fzu} email: logic_fzu@outlook.com, All Rights Reserved.
 */

#include <stdio.h>
#include <string.h>
#include <unistd.h>

#include "cmsis_os2.h"
#include "hi_uart.h"
#include "hi_io.h"


//向外提供的发送接口
void scs_uart2_init(void)
{
   uint32_t  ret;

    //初始化串口2
    hi_io_set_func(HI_IO_NAME_GPIO_11, HI_IO_FUNC_GPIO_11_UART2_TXD); /* uart2 tx */
    hi_io_set_func(HI_IO_NAME_GPIO_12, HI_IO_FUNC_GPIO_12_UART2_RXD); /* uart2 rx */
    hi_uart_attribute uart2_attr = {
        .baud_rate = 1000000,
        .data_bits = 8,
        .stop_bits = 1,
        .parity = 0,
    };

	ret = hi_uart_init(HI_UART_IDX_2, &uart2_attr, NULL);
    printf("init uart2 code = %d\n", ret);
    if (ret != HI_ERR_SUCCESS)
    {
        printf("Failed to init uart2! Err code = %d\n", ret);
    }	
	
}

//入口参数           舵机对应ID    舵机控制数量    舵机位置信息数组  需要与ID对应
void SnycWrite(uint8_t ID[], uint8_t IDnumber,  int *nDat)
{
	uint8_t i;
	uint8_t data_len= 2;
	uint16_t checkSum = 0;
	uint8_t checkSum_low = 0;
	uint8_t mesLen = ((data_len+1)*IDnumber+4);
	uint8_t buffLen = (IDnumber*3+8);
	uint8_t write_Buf[buffLen];
	write_Buf[0] = 0xff;
	write_Buf[1] = 0xff;
	write_Buf[2] = 0xfe;     //广播ID  254(0XFE)  所有舵机都能收到
	write_Buf[3] = mesLen;   //(L + 1) * N + 4 (L: 发给每个舵机的数据长度, N: 舵机的个数)  
	write_Buf[4] = 0x83;     //指令
	write_Buf[5] = 0x2a;     //写入数据的首地址0X2A
	write_Buf[6] = data_len; //写入的数据的长度(L)   支持位置控制，pwm 速度控制，不支持总线速度控制  有阻尼模式  上电自由模式
	checkSum = 0xfe + mesLen + 0x83 +0x2a + data_len;
	for(i=0; i<IDnumber; i++){
	write_Buf[7+3*i] = ID[i];
	write_Buf[8+3*i] = nDat[i] >> 8;      //右移动   高八位  低位字节在前，高位节位在后
	write_Buf[9+3*i] = nDat[i];           //低八位    		
	checkSum += write_Buf[7+3*i];
	checkSum += write_Buf[8+3*i];	
	checkSum += write_Buf[9+3*i];
	}
	checkSum_low= ~checkSum;
	write_Buf[buffLen-1]=checkSum_low;
    hi_uart_write_immediately(HI_UART_IDX_2, write_Buf,buffLen);//串口发送函数  将write_Buf发送至驱动板  
}
